﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <xy/geom/quat.h>

using namespace xy;
using namespace xy::geom;

TEST(TestQuat, basic)
{
    {
        quat<double> qa(2.0, -2.0, 3.0, -4.0);
        quat<double> qb(1.0, -2.0, 5.0, -6.0);
        EXPECT_EQ(qa * qb, quat<double>(-41.0, -4.0, 9.0, -20.0));
        EXPECT_EQ(qb * qa, quat<double>(-41.0, -8.0, 17.0, -12.0));
        EXPECT_EQ(qa.conjugate(), quat<double>(2.0, 2.0, -3.0, 4.0));
    }
}

TEST(TestQuat, rotation)
{

    {
        vec3 v(1.0 / sqrt(2), 0.0, 1.0 / sqrt(2));
        quat<double> q = quat<double>::rotate(v, math::numeric::pi / 2);
        quat<double> p(0.0, 2.0, 0.0, 0.0);

        // 逆时针绕 v 转 90 度
        auto pp = q * p * q.conjugate();
        EXPECT_EQ(pp, quat<double>(0.0, 1.0, sqrt(2), 1.0));

        // 从一个轴转到另一个轴
        vec3 v1(2.4, 3.6, 5.8);
        v1.normalize();
        vec3 v2(5.1, 9.6, -2.7);
        v2.normalize();
        quat<double> q2 = quat<double>::from_to(v1, v2);
        EXPECT_LE((q2 * v1 - v2).modulus(), 1e-6);
    }

    // 当转角 theta / 2 小于 45 度时，矩阵迹 > 0
    {
        vec3 v(1.0 / sqrt(2), 0.0, 1.0 / sqrt(2));
        quat<double> q = quat<double>::rotate(v, math::numeric::pi / 3);
        auto m = quat<double>::to_mat4x4(q);
        auto q1 = quat<double>::from_mat4x4(m);

        vec3 o(1.0, 2.0, 3.0);
        EXPECT_LE((q * o - q1 * o).modulus(), 1e-6);
    }

    // 当转角 theta / 2 大于 45 度时，矩阵迹 < 0
    // 分别测试最大元素位于 m(0, 0), m(1, 1), m(2, 2) 三个位置
    {
        vec3 v(1.0, 0.0, 0.0);
        quat<double> q = quat<double>::rotate(v, math::numeric::pi / 1.5);
        auto m = quat<double>::to_mat4x4(q);
        auto q1 = quat<double>::from_mat4x4(m);

        vec3 o(1.0, 2.0, 3.0);
        EXPECT_LE((q * o - q1 * o).modulus(), 1e-6);
    }

    {
        vec3 v(0.0, 1.0, 0.0);
        quat<double> q = quat<double>::rotate(v, math::numeric::pi / 1.5);
        auto m = quat<double>::to_mat4x4(q);
        auto q1 = quat<double>::from_mat4x4(m);

        vec3 o(1.0, 2.0, 3.0);
        EXPECT_LE((q * o - q1 * o).modulus(), 1e-6);
    }

    {
        vec3 v(0.0, 0.0, 1.0);
        quat<double> q = quat<double>::rotate(v, math::numeric::pi / 1.5);
        auto m = quat<double>::to_mat4x4(q);
        auto q1 = quat<double>::from_mat4x4(m);

        vec3 o(1.0, 2.0, 3.0);
        EXPECT_LE((q * o - q1 * o).modulus(), 1e-6);
    }

    {
        auto q = quat<double>::euler(0.074, -178.889, -10.318);
        auto v = q.to_euler();
        std::cout << q << std::endl;
        std::cout << v << std::endl;
    }
}